//
// Created by rebeater on 3/24/21.
//
#include <string.h>
#include <stdarg.h>
#include "usart.h"
#include "tim.h"
#include "stdio.h"
#include "nav_struct.h"
#include "math.h"
#include "rtcommon.h"
#include "main.h"

#ifdef __cplusplus
extern "C" {
#endif
#ifdef __GNUC__
#define IO_PUT_CHAR_DECLARATION  int\
            __io_putchar(int ch)
#else
#define IO_PUT_CHAR_DECLARATION int\
	f_putc(int ch)
#endif

IO_PUT_CHAR_DECLARATION {
  /**/
  HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);
  return ch;
}

#ifdef __cplusplus
}
#endif
#define _STR(s) #s
#define STR(s) _STR(s)
#if GNSS_RCV == GNSS_RCV_UB482
#define GNSS_DEVICE_NAME UB482
#else
#define GNSS_DEVICE_NAME MXT906B
#endif
#if IMU_DEVICE == IMU_KYIMU102
#define IMU_DEVICE IMU_KYIMU102
#elif IMU_DEVICE == IMU_ADIS16460
#define IMU_DEVICE_NAME ADIS16460
#else
#define IMU_DEVICE_NAME ICM20602
#endif
char Version[] = STR(MAJOR_VERSION.MINOR_VERSION);
const char *Description=STR(\n-----------------------------------------------------
	\nGNSS/INS Integrated Navigation System based on ARM(R)
	\nVersion: MAJOR_VERSION.MINOR_VERSION.build.BUILD_VERSION
	\nHardware: GNSS: GNSS_DEVICE_NAME & IMU_DEVICE_NAME
	\n------------------------------------------------------\n);


#if GNSS_RCV == GNSS_RCV_MXT906B
char GnssDeviceName[] = "MXT906B";
#elif GNSS_RCV == GNSS_RCV_UB482
char GnssDeviceName[] = "UB482";
#endif
#if IMU_DEVICE == IMU_ADIS16460
char ImuName[] = "ADIS16460";
char ImuFileNameFormat[] = "ADI_%02d%02d%02d_%06d.adi";
#elif IMU_DEVICE == IMU_KYIMU102
char ImuName[] = "KY-IMU102-A0";
char ImuFileNameFormat[] = "KY_%02d%02d%02d_%06d.ky";
#elif IMU_DEVICE == IMU_ICM20602
char ImuFileNameFormat[] = "IMU_%02d%02d%02d_%06d.iraw";
#if ENABLE_MULTI_IMUS == 1
char ImuName[] = "ICM20602_4";
#else
char ImuName[] = "ICM20602";
#endif
#elif IMU_DEVICE == IMU_STIM300
char ImuName[] = "STIM300";
#endif
extern uint64_t pps_margin;
extern uint32_t system_time_int;


 char kUart6TxBuffer[512] ALIGN_RAM_D1;
int trace(TraceLevel level,const char *fmt, ...) {
  va_list ap;
  int retval;
  switch (level) {
	case Info:sprintf(kUart6TxBuffer, "$I,");
	  break;
	case Warning:sprintf(kUart6TxBuffer, "$W,");
	  break;
	case Error:sprintf(kUart6TxBuffer, "$E,");
	  break;
	case Fetal:sprintf(kUart6TxBuffer, "$F,");
		Error_Handler();
	  break;
	default:break;
  }
  va_start(ap, fmt);
  retval = vsprintf(kUart6TxBuffer + 3, fmt, ap);
  va_end(ap);
//    SCB_CleanInvalidateDCache_by_Addr((uint32_t *)kUart6TxBuffer, 512);
  HAL_UART_Transmit(&huart6, (uint8_t *)kUart6TxBuffer, strlen(kUart6TxBuffer),0xFFF);
  return retval;
}


void printDouble(double d, uint8_t n) {
  if (d < 0) {
	printf("-");
	printDouble(-d, n);
	return;
  }
  int di = (int)d;
  double dd = d - di;
  uint64_t ddi = (uint64_t)(pow(10, n) * dd);
  char buff[10];
  sprintf(buff, "%%d.%%0%dlu", n);
  printf(buff, di, ddi);
}

void sprintDouble(char *buffer, double d, uint8_t n) {
  static double _10e[] = {1, 1e1, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10, 1e11, 1e12, 1e13};
  if (d < 0) {
	sprintf(buffer, "-");
	sprintDouble(buffer + 1, -d, n);
	return;
  }
  int di = (int)d;
  double dd = d - di;
  uint64_t ddi = (uint64_t)(_10e[n] * dd);
  char buff[16];
  sprintf(buff, "%%d.%%0%dllu,", n);
  sprintf(buffer, buff, di, ddi);
}

extern double system_time_start;

/**
 * 从寄存器读取当前时刻，并转换为GPS时间
 * @return
 */
double GetCurrentGpst(){
  return system_time_start + system_time_int + htim2.Instance->CNT /(double) pps_margin;
}

void sprintOutput(char *buffer,short week,int gpst, NavOutput *out){
  char *p = buffer;
  sprintf(p, "$POS,");
  while (*p != '\0') p++;
  sprintf(p,"%04d,", week);
  while (*p != '\0') p++;
  sprintDouble(p, gpst, 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->lat, 12);
  while (*p != '\0')p++;
  sprintDouble(p, out->lon, 12);
  while (*p != '\0')p++;
  sprintDouble(p, out->height, 3);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[0], 2);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[1], 2);
  while (*p != '\0')p++;
  sprintDouble(p, out->vn[2], 2);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[0], 1);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[1], 1);
  while (*p != '\0')p++;
  sprintDouble(p, out->atti[2], 1);
  while (*p != '\0')p++;
/*  for (int i = 0; i < 3; i++) {
	sprintf(p, "%d,", (int)(out->ab[i]));
	while (*p != '\0')p++;
  }
  for (int i = 0; i < 3; i++) {
	sprintf(p, "%d,", (int)(out->gb[i]));
	while (*p != '\0')p++;
  }
  for (int i = 0; i < 3; i++) {
	sprintDouble(p, (out->pos_std[i]), 2);
	while (*p != '\0')p++;
  }
  for (int i = 0; i < 3; i++) {
	sprintDouble(p, (1e4 * out->atti_std[i]), 2);
	while (*p != '\0')p++;
  }
  sprintf(p, "%3d,", (int)(100 * out->kd));
  while (*p != '\0')p++;*/
  /*  sprintDouble(p, out->ab[0], 1);
	while (*p != '\0')p++;
	sprintDouble(p, out->ab[1], 1);
	while (*p != '\0')p++;
	sprintDouble(p, out->ab[2], 1);
	while (*p != '\0')p++;
	sprintDouble(p, out->gb[0], 1);
	while (*p != '\0')p++;
	sprintDouble(p, out->gb[1], 1);
	while (*p != '\0')p++;
	sprintDouble(p, out->gb[2], 1);*/
  sprintf(p, "%02d", out->info.mode);
  while (*p != '\0')p++;
  sprintf(p, "%02d", out->info.sensors);
  while (*p != '\0')p++;
  *p = '\n';
  p++;
  *p = '\0';
}
#define CRC32_POLY 0x04C11DB7U
#define CRC32_START 0xFFFFFFFFU
uint32_t crc_checksum(const uint8_t *indata, int num_bytes) {
#if HARDWARE_CRC == 1
  /*在STM32中通过硬件硬件实现*/
#else
  /*软件校验*/
  uint32_t crc = CRC32_START;
  const uint8_t *p = indata;
  for (int i = 0; i < num_bytes; i++) {
	crc = crc ^ (*p++ << 24U);
	for (int bit = 0; bit < 8; bit++) {
	  if (crc & (1LU << 31U)) crc = (crc << 1U) ^ CRC32_POLY;
	  else crc = (crc << 1U);
	}
  }
#endif
  return crc;
}

/*! 1Hz輸出任务 */
#if 0
void printOutput(NavOutput *out) {
  printDouble(out->lat / _deg, 12);
  printf(" ");
  printDouble(out->lon / _deg, 12);
  printf(" ");
  printDouble(out->height, 3);
  printf(" ");
  printDouble(out->vn[0], 3);
  printf(" ");
  printDouble(out->vn[1], 3);
  printf(" ");
  printDouble(out->vn[2], 3);
  printf(" ");
  printDouble(out->atti[0] / _deg, 3);
  printf(" ");
  printDouble(out->atti[1] / _deg, 3);
  printf(" ");
  printDouble(out->atti[2] / _deg, 3);
  printf("\n");
}
#endif
/*leap year*/
int is_leap_year(int year) {
  if ((year % 400 == 0) || (year % 4 == 0 && year % 100 != 0)) {
	return 1;
  } else {
	return 0;
  }
}
/**
 * 计算当前日期到1970年1月6日所经过的天数。
 * @param year : 2017
 * @param mon : 4
 * @param day :12
 * @return
 */
int day_from_gps0(const int year, const int mon, const int day) {
  int32_t months[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int32_t months_leap[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int32_t days = -6;
  for (int y = 1980; y < year; y++) {
	days += (365 + is_leap_year(y));
  }
  if (is_leap_year(year)) {
	for (int m = 0; m < mon - 1; m++) {
	  days += months_leap[m];
	}
  } else {
	for (int m = 0; m < mon - 1; m++) {
	  days += months[m];
	}
  }
  days += day;
  return days;
}

/*GPS时间转换为UTC时间*/
void gpst_to_utc(int week, double gpst, DateTime *time) {
  unsigned short months[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  unsigned short months_leap[] = {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  int days = week * 7 + (int)(gpst / 86400.0);
  time->year = (int)(days / 365);
  int doy = days - day_from_gps0(time->year + 1980, 0, 0);
  if (doy < 0) {
	time->year--;
	doy = days - day_from_gps0(time->year + 1980, 0, 0);
  }
  unsigned short *p_month;
  if (is_leap_year(time->year + 1980)) {
	p_month = months_leap;
  } else {
	p_month = months;
  }
  int i;
  for (i = 0; i < 12; i++) {
	doy -= p_month[i];
	if (doy < 0) {
	  break;
	}
  }
  time->mon = i + 1;
  time->day = doy + p_month[i];

  /*算时间*/
  double sod = gpst - 86400 * (int)(gpst / 86400);
  time->hour = (int)(sod / 3600);
  sod -= (3600 * time->hour);
  time->minute = (int)((sod) / 60);
  sod -= (60 * time->minute);
  time->second = (int)sod;
}
